#include "turn_on_node.h"

int main(int argc, char** argv)
{
    ros::init(argc, argv, "turn_on_node");
    TurnOnNode node;
    node.init();
    node.run();
    return 0;
}

TurnOnNode::TurnOnNode()
{
    // Constructor implementation
}

TurnOnNode::~TurnOnNode()
{
    // Destructor implementation
}

void TurnOnNode::init()
{
    // Initialization code
    ros::NodeHandle nh;
    carto_sub = nh.subscribe<geometry_msgs::PoseStamped>("/tracked_pose", 1, &TurnOnNode::GetCartographPoseCmdCallback, this);
    params = std::make_shared<Params>();
    tools = std::make_shared<Tools>();
    navController = std::make_shared<NavigationController>(params);
    pclMatch = std::make_shared<PclMatch>(params);

}


/**
 * @brief 调试指令回调入口
 * @param msg-指令字符串
 * @return
 * */
void TurnOnNode::GetTestCmdCallback(const std_msgs::String::ConstPtr& msg)
{
    int ret = 0;
    float spin = 0.0;
    ACTION_DATA_S action;
    ROS_WARN("get test cmd[%s]", msg->data.c_str());
    if (strstr(msg->data.c_str(), "docking") != NULL) {
        ros::Rate r(20);
        ros::NodeHandle n;
        // 计算直角交点(x0,y0)=1.862341, 1.191646，目标点 1.36， 0.71
        params->get_cross_point_flag_ = true;
        // 设置到导航点到（目标点附近）
        navController->PublishGoalCmd(1.36, 0.31, 3.14);
        // 自定义对接运动
        while (n.ok()) {
            ret = navController->DockingTaskAction_WithPCL(&action,g_pose_2d, params->g_cross_point_);
            if (!ret)
                navController->PublishMotorCmd(action.v, action.w);//不检测尾部距离
            else {
                navController->PublishMotorCmd(0, 0);
                break;
            }
            ros::spinOnce();
            r.sleep();
        }
        params->get_cross_point_flag_ = false;
    }
}

void TurnOnNode::run()
{
    //定义ros nh
    ros::NodeHandle nh;
    ros::Subscriber test_sub = nh.subscribe("/dm_test", 1, &TurnOnNode::GetTestCmdCallback, this);
}

/**
 * @brief 获取carto位置信息
 * @param pose-当前坐标
 * @return
 * */
int pos_count = 0;
void TurnOnNode::GetCartographPoseCmdCallback(const geometry_msgs::PoseStamped::ConstPtr& pose)
{
    g_carto_pose_.pose = pose->pose;
    g_pose_2d.x = g_carto_pose_.pose.position.x;
    g_pose_2d.y = g_carto_pose_.pose.position.y;
    g_pose_2d.yaw = tools->QuaternionTransToYaw(pose->pose.orientation);


    pos_count++;
    if (pos_count % 8 == 0) //N次发一次位置
    {
        pos_count = 0;
        PublishPoseCmd(g_carto_pose_.pose.position.x, g_carto_pose_.pose.position.y, g_pose_2d.yaw * RADIAN_TO_DEGREE);
    }
}

/**
 * @brief 发布位置信息
 * @param (x, y) - 坐标
 * 		  a - 角度
 * @return
 * */
void TurnOnNode::PublishPoseCmd(float x, float y, float a)
{
    turn_on_robot::DMPose pose;
    pose.x = x;
    pose.y = y;
    pose.angle = a;
    pub_pose_.publish(pose);
}

